#!/usr/bin/env python3
import rospy
import numpy as np
from geometry_msgs.msg import PointStamped
from Kalman import KalmanFilter as KF


class ball_predict:
    def __init__(self):
        rospy.init_node('predict_ball')
        self.position_before_array = []
        self.position_before_array.append(0.0000)
        self.position_before_array.append(0.0000)
        self.position_before_array.append(0.0000)
        self.position_predict_array = []
        self.position_predict_array.append(0.0000)
        self.position_predict_array.append(0.0000)
        self.position_predict_array.append(0.0000)
        self.kf = KF()

    def ball_publisher(self):
        '''
        @ publish 将预测的信息发布

        '''
        pub = rospy.Publisher('/predict_position', PointStamped, queue_size=1)
        predict_point_stamp = PointStamped()
        current_time = rospy.Time.now()
        predict_point_stamp.header.stamp = current_time
        predict_point_stamp.header.frame_id = "odom"
        predict_point_stamp.point.x = self.position_predict_array[0]
        predict_point_stamp.point.y = self.position_predict_array[1]
        predict_point_stamp.point.z = self.position_predict_array[2]
        rospy.loginfo('predict_x: {:.4f}'.format(
            self.position_predict_array[0]))
        rospy.loginfo('predict_y: {:.4f}'.format(
            self.position_predict_array[1]))
        rospy.loginfo('predict_z: {:.4f}'.format(
            self.position_predict_array[2]))
        pub.publish(predict_point_stamp)

    def ball_subscriber(self):
        '''
        @ subscribe 位置信息
        '''
        rospy.Subscriber("/position", PointStamped, self.pointInfoCallback)
        rospy.spin()

    def pointInfoCallback(self, msg):
        '''
        @ 回调函数
            将获得到的位置信息载入
        position_predict_x
        position_predict_y
        position_predict_z
        '''
        self.position_before_array[0] = msg.point.x
        self.position_before_array[1] = msg.point.y
        self.position_before_array[2] = msg.point.z
        array = self.kf.process(self.position_before_array)
        self.position_predict_array = array
        '''
        @ 调用publish
        '''
        self.ball_publisher()


if __name__ == '__main__':
    predict = ball_predict()
    predict.ball_subscriber()
